#
# roomba.py
#
# The Erdos library for the roomba
#
# Zach Dodds   dodds@cs.hmc.edu
#
# 3/1/2006
# 5/10/2006: supports all three sensor-reading frames, odometry
#
# You will need pyserial 
#    It comes with win32all, also available for MacOSX and Linux.
#
# To get startd at the command line:
# import roomba
# r = roomba.Roomba(1) # if it's COM1 on the PC
# r = roomba.Roomba('/dev/tty.RooTooth-COM0-1')  # for my Mac
# print r.readSensors()
# r.getMode()
# roomba.modeStr(r.getMode())
# 
# r.toSafeMode()
# r.toFullMode()  # if you want
# 
# r.Go(10,0)   # cm per sec, deg per sec
# r.drive( 50, 30000 )
# r.drive( 0, 30000 )
# 
# r.setLED( roomba.LED_STATUS, 1 )
# r.setLED( roomba.LED_STATUS, 2 ) # 0-3
# r.setLED( roomba.LED_SPOT, 1 ) 
# r.setLED( roomba.LED_CLEAN, 1 ) 
# r.setLED( roomba.LED_MAX, 1 )    # not on roomba red
# r.setLED( roomba.LED_DIRTDETECT, 1 )
# r.setLED( roomba.LED_POWER, 255, 42 )
# 
# r.playNote( 60, 64 )   # middle C, one second
# r.setSong(0, [(60,32), (60,32), (67,32), (67,32), (69,32), (69,32), (67, 64)])
# r.playSong(0)
# 
# r.setVacuum(1)
# r.setSideBrush(1)
# r.setMainBrush(1)
# r.setAbsoluteMotors()
# 
# r.powerOff()
# 
# r.close()
# 
# r.closeSerial()
# r.openSerial()
# 
# r.flush()        # not implemented yet
#

import serial
import math
import time

# some module-level definitions for the robot commands
START = chr(128)    # already converted to bytes...
BAUD = chr(129)     # + 1 byte
CONTROL = chr(130)
SAFE = chr(131)
FULL = chr(132)
POWER = chr(133)
SPOT = chr(134)
CLEAN = chr(135)
MAX = chr(136)
DRIVE = chr(137)    # + 4 bytes
MOTORS = chr(138)   # + 1 byte
LEDS = chr(139)     # + 3 bytes
SONG = chr(140)     # + 2N+2 bytes, where N is the number of notes
PLAY = chr(141)     # + 1 byte
SENSORS = chr(142)  # + 1 byte
FORCESEEKINGDOCK = chr(143)

# the LEDs
LED_STATUS = 0
LED_SPOT = 1
LED_CLEAN = 2
LED_MAX = 3
LED_DIRTDETECT = 4
LED_POWER = 5

# the motors
MOTOR_MAINBRUSH = 0
MOTOR_VACUUM = 1
MOTOR_SIDEBRUSH = 2

# the four SCI modes
# the code will try to keep track of which mode the system is in,
# but this might not be 100% trivial...
OFF_MODE = 0
PASSIVE_MODE = 1
SAFE_MODE = 2
FULL_MODE = 3

# for printing the SCI modes
def modeStr( mode ):
    """ prints a string representing the input SCI mode """
    if mode == OFF_MODE: return 'OFF_MODE'
    if mode == PASSIVE_MODE: return 'PASSIVE_MODE'
    if mode == SAFE_MODE: return 'SAFE_MODE'
    if mode == FULL_MODE: return 'FULL_MODE'
    print 'Warning: unknown mode', mode, 'seen in modeStr'
    return 'UNKNOWN_MODE'

#
# some module-level functions for dealing with bits and bytes
#
def bytesOfR( r ):
    """ for looking at the raw bytes of a sensor reply, r """
    print 'raw r is', r
    for i in range(len(r)):
        print 'byte', i, 'is', ord(r[i])
    print 'finished with formatR'

def bitOfByte( bit, byte ):
    """ returns a 0 or 1: the value of the 'bit' of 'byte' """
    if bit < 0 or bit > 7:
        print 'Your bit of', bit, 'is out of range (0-7)'
        print 'returning 0'
        return 0
    return ((byte >> bit) & 0x01)

def toBinary( val, numbits ):
    """ prints numbits digits of val in binary """
    if numbits == 0:  return
    toBinary( val>>1 , numbits-1 )
    print (val & 0x01),  # print least significant bit


def fromBinary( s ):
    """ s is a string of 0's and 1's """
    if s == '': return 0
    lowbit = ord(s[-1]) - ord('0')
    return lowbit + 2*fromBinary( s[:-1] )
    

def twosComplementInt1byte( byte ):
    """ returns an int of the same value of the input
        int (a byte), but interpreted in two's
        complement
        the output range should be -128 to 127
    """
    # take everything except the top bit
    topbit = bitOfByte( 7, byte )
    lowerbits = byte & 127
    if topbit == 1:
        return lowerbits - (1 << 7)
    else:
        return lowerbits


def twosComplementInt2bytes( highByte, lowByte ):
    """ returns an int which has the same value
        as the twosComplement value stored in
        the two bytes passed in

        the output range should be -32768 to 32767

        chars or ints can be input, both will be
        truncated to 8 bits
    """
    # take everything except the top bit
    topbit = bitOfByte( 7, highByte )
    lowerbits = highByte & 127
    unsignedInt = lowerbits << 8 | (lowByte & 0xFF)
    if topbit == 1:
        # with sufficient thought, I've convinced
        # myself of this... we'll see, I suppose.
        return unsignedInt - (1 << 15)
    else:
        return unsignedInt


def toTwosComplement2Bytes( value ):
    """ returns two bytes (ints) in high, low order
        whose bits form the input value when interpreted in
        two's complement
    """
    # if positive or zero, it's OK
    if value >= 0:
        eqBitVal = value
    # if it's negative, I think it is this
    else:
        eqBitVal = (1<<16) + value

    return ( (eqBitVal >> 8) & 0xFF, eqBitVal & 0xFF )

def poseDeltaFromVelRadSec( vel_mm_sec, ROC, sec ):
    """ returns the pose change (dx,dy,dthr) in (mm,mm,radians)
        undergone by a differential-drive robot 
        with a wheelspan of 258mm that is traveling with
        a "velocity" of vel_mm_sec, along a radius of
        ROC_mm, for sec seconds

        NOTE that the pose change is represented in the canonical
        "robot-centric" coordinate system:

              Hooray for ASCII art!
            
                            | +x         aligned to robot's heading
                            |
                        ^   |   ^  
                        |   |   |
                 +y <---WL--+--WR--- -y  perp to robot's heading
                            |
                            |            DELTA = 1/2 distance from WL to WR
                            | -x
            
             vel_mm_sec is the average of the velocities of WL and WR
             it is positive when the robot is moving forward
           
             the center of the robot's circular arc is at (0,ROC)
             positive ROC => turning to the left
             negative ROC => turning to the right
             
        Special cases: ROC ==  1    => counterclockwise
                       ROC == -1    => clockwise
                       ROC == 32768 => straight
    """
    # the robot moves along the arc of a circle
    #
    #     the robot's position after the arc is
    #     (0,ROC) + (ROC*sin(thr),-ROC*cos(thr))
    #
    # so we first find thr
    #
    # handle special cases
    #

    DELTA = 258.0/2.0   # there are 258 mm between the roomba's wheels

    if ROC == 32768:
        # going straight, so the wheels have equal velocities
        # and there is no angular change
        thr_delta = 0.0
        x_delta = vel_mm_sec * sec   # D = RT in action!
        y_delta = 0.0

    elif ROC == 1 or ROC == 0:
        # turning in place counterclockwise = positive thr_delta
        x_delta = 0.0
        y_delta = 0.0
        # to do - check if the sign of vel_mm_sec matters!
        thr_delta = (vel_mm_sec * sec)/float(DELTA)

    elif ROC == -1:
        # turning in place counterclockwise = positive thr_delta
        x_delta = 0.0
        y_delta = 0.0
        # to do - check if the sign of vel_mm_sec matters!
        thr_delta = - ( (vel_mm_sec * sec)/float(DELTA) )

    else:
        # general case
        # how much distance did the center travel
        # the specification is not 100% clear whether vel_mm_sec
        # is the average velocity (signed) or speed (unsigned)
        # of the wheels... we need to test this!

        # we assume average speed (unsigned) for now...
        # we handle the case where vel_mm_sec and ROC are both > 0
        #    and then check for signs later...
        pos_vel_mm_sec = math.fabs(vel_mm_sec)
        pos_ROC = math.fabs(ROC)

        # need to switch the sign of the left wheel when the ROC < DELTA
        if DELTA <= pos_ROC:
            # both wheels are going the same direction
            # center is traveling at pos_vel_mm_sec
	    # center is traveling at a radians/second 
	    a =  pos_vel_mm_sec / pos_ROC
        else:
            # wheels going in opposite directions
            # center is traveling at a radians/second
            a = pos_vel_mm_sec / DELTA

	# we find the total (positive) angle traveled, pos_thr
	pos_thr = a * sec

	# we handle four different cases

	# case 1: ROC >= 0 and vel_mm_sec >= 0  (forward left)
	if ROC >= 0 and vel_mm_sec >= 0:
	    thr_delta = pos_thr
            # (0,ROC) + (ROC*sin(thr_delta),-ROC*cos(thr_delta))
	    x_delta = 0.0 + ROC*math.sin(thr_delta)
	    y_delta = ROC - ROC*math.cos(thr_delta)

	# case 2: ROC <  0 and vel_mm_sec >= 0  (forward right)
	if ROC <  0 and vel_mm_sec >= 0:
	    thr_delta = -pos_thr
            # (0,ROC) + (ROC*sin(thr_delta),-ROC*cos(thr_delta))
	    x_delta = 0.0 + ROC*math.sin(thr_delta)
	    y_delta = ROC - ROC*math.cos(thr_delta)

	# case 3: ROC >= 0 and vel_mm_sec <  0  (backward left)
	if ROC >= 0 and vel_mm_sec <  0:
	    thr_delta = -pos_thr
            # (0,ROC) + (ROC*sin(thr_delta),-ROC*cos(thr_delta))
	    x_delta = 0.0 + ROC*math.sin(thr_delta)
	    y_delta = ROC - ROC*math.cos(thr_delta)

	# case 4: ROC <  0 and vel_mm_sec <  0  (backward right)
	if ROC <  0 and vel_mm_sec <  0:
	    thr_delta = pos_thr
            # (0,ROC) + (ROC*sin(thr_delta),-ROC*cos(thr_delta))
	    x_delta = 0.0 + ROC*math.sin(thr_delta)
	    y_delta = ROC - ROC*math.cos(thr_delta)


    return (x_delta, y_delta, thr_delta)


# 
# this class represents a snapshot of the robot's data
#
class SensorFrame:
    """ the sensorFrame class is really a struct whose
        fields are filled in by sensorStatus
    """

    def __init__(self):
        """ constructor -- set all fields to 0 

            see interpretSensorString for details
            on all of these fields
        """
        self.casterDrop = 0
        self.leftWheelDrop = 0
        self.rightWheelDrop = 0
        self.leftBump = 0
        self.rightBump = 0
        self.wallSensor = 0
        self.leftCliff = 0
        self.frontLeftCliff = 0
        self.frontRightCliff = 0
        self.rightCliff = 0
        self.virtualWall = 0
        self.driveLeft = 0
        self.driveRight = 0
        self.mainBrush = 0
        self.vacuum = 0
        self.sideBrush = 0
        self.leftDirt = 0
        self.rightDirt = 0
        self.remoteControlCommand = 0
        self.powerButton = 0
        self.spotButton = 0
        self.cleanButton = 0
        self.maxButton = 0
        self.distance = 0
        self.rawAngle = 0
        self.angleInRadians = 0
        self.chargingState = 0
        self.voltage = 0
        self.current = 0
        self.temperature = 0
        self.charge = 0
        self.capacity = 0

    def __str__(self):
        """ returns a string with the information
            from this SensorFrame
        """
        # there's probably a more efficient way to do this...
        # perhaps just making it all + instead of the separate
        # += would be more efficient
        #
        # actually, we should make a list and call ''.join(list)
        # not that we will...
        #
        s = ''
        s += 'casterDrop: ' + str(self.casterDrop) + '\n'
        s += 'leftWheelDrop: ' + str(self.leftWheelDrop) + '\n'
        s += 'rightWheelDrop: ' + str(self.rightWheelDrop) + '\n'
        s += 'leftBump: ' + str(self.leftBump) + '\n'
        s += 'rightBump: ' + str(self.rightBump) + '\n'
        s += 'wallSensor: ' + str(self.wallSensor) + '\n'
        s += 'leftCliff: ' + str(self.leftCliff) + '\n'
        s += 'frontLeftCliff: ' + str(self.frontLeftCliff) + '\n'
        s += 'frontRightCliff: ' + str(self.frontRightCliff) + '\n'
        s += 'rightCliff: ' + str(self.rightCliff) + '\n'
        s += 'virtualWall: ' + str(self.virtualWall) + '\n'
        s += 'driveLeft: ' + str(self.driveLeft) + '\n'
        s += 'driveRight: ' + str(self.driveRight) + '\n'
        s += 'mainBrush: ' + str(self.mainBrush) + '\n'
        s += 'vacuum: ' + str(self.vacuum) + '\n'
        s += 'sideBrush: ' + str(self.sideBrush) + '\n'
        s += 'leftDirt: ' + str(self.leftDirt) + '\n'
        s += 'rightDirt: ' + str(self.rightDirt) + '\n'
        s += 'remoteControlCommand: ' + str(self.remoteControlCommand) + '\n'
        s += 'powerButton: ' + str(self.powerButton) + '\n'
        s += 'spotButton: ' + str(self.spotButton) + '\n'
        s += 'cleanButton: ' + str(self.cleanButton) + '\n'
        s += 'maxButton: ' + str(self.maxButton) + '\n'
        s += 'distance: ' + str(self.distance) + '\n'
        s += 'rawAngle: ' + str(self.rawAngle) + '\n'
        s += 'angleInRadians: ' + str(self.angleInRadians) + '\n'
        # no data member needed for this next line
        s += 'angleInDegrees: ' + str(math.degrees(self.angleInRadians)) + '\n'
        s += 'chargingState: ' + str(self.chargingState) + '\n'
        s += 'voltage: ' + str(self.voltage) + '\n'
        s += 'current: ' + str(self.current) + '\n'
        s += 'temperature: ' + str(self.temperature) + '\n'
        s += 'charge: ' + str(self.charge) + '\n'
        s += 'capacity: ' + str(self.capacity) + '\n'
        return s

    def toBinaryString(self):
        """ this converts the calling SensorFrame into a 26-byte
            string of the format the roomba sends back
        """
        # todo: handle the different subsets (frames) of sensor data

        # here are the 26 bytes in list form
        slist = [0]*26

        # First Frame

        # byte 0: bumps and wheeldrops
        slist[0] = self.casterDrop << 4 | \
                   self.leftWheelDrop << 3 | \
                   self.rightWheelDrop << 2 | \
                   self.leftBump << 1 | \
                   self.rightBump

        # byte 1: wall data
        slist[1] = self.wallSensor

        # byte 2: cliff left
        slist[2] = self.leftCliff
        # byte 3: cliff front left
        slist[3] = self.frontLeftCliff
        # byte 4: cliff front right
        slist[4] = self.frontRightCliff
        # byte 5: cliff right
        slist[5] = self.rightCliff

        # byte 6: virtual wall
        slist[6] = self.virtualWall

        # byte 7: motor overcurrents
        slist[7] = self.driveLeft << 4 | \
                   self.driveRight << 3 | \
                   self.mainBrush << 2 | \
                   self.vacuum << 1 | \
                   self.sideBrush

        # byte 8: dirt detector left
        slist[8] = self.leftDirt
        # byte 9: dirt detector left
        slist[9] = self.rightDirt

        # Second Frame
        
        # byte 10: remote control command
        slist[10] = self.remoteControlCommand

        # byte 11: buttons
        slist[11] = self.powerButton << 3 | \
                    self.spotButton << 2 | \
                    self.cleanButton << 1 | \
                    self.maxButton

        # bytes 12, 13: distance
        highVal, lowVal = toTwosComplement2Bytes( self.distance )
        slist[12] = highVal
        slist[13] = lowVal

        # bytes 14, 15: angle
        highVal, lowVal = toTwosComplement2Bytes( self.rawAngle )
        slist[14] = highVal
        slist[15] = lowVal

        # Third Frame

        # byte 16: charging state
        slist[16] = self.chargingState

        # bytes 17, 18: voltage
        slist[17] = (self.voltage >> 8) & 0xFF
        slist[18] = self.voltage & 0xFF

        # bytes 19, 20: current
        highVal, lowVal = toTwosComplement2Bytes( self.current )
        slist[19] = highVal
        slist[20] = lowVal

        # byte 21: temperature
        slist[21] = self.temperature

        # bytes 22, 23: charge
        slist[22] = (self.charge >> 8) & 0xFF
        slist[23] = self.charge & 0xFF

        # bytes 24, 25: capacity
        slist[24] = (self.capacity >> 8) & 0xFF
        slist[25] = self.capacity & 0xFF

        # convert to a string
        s = ''.join([ chr(x) for x in slist ])

        return s



#
# the robot class
#
class Roomba:
    """ the Roomba class is an abstraction of the Roomba's
        SCI interface, including communication and a bit
        of processing of the strings passed back and forth
 
        when you create an object of type Roomba, the code
        will try to open a connection to it - so, it will fail
        if it's not attached!
 
        Right now, the code assumes the SCI begins in "off" mode
        (listening for the Start command), which is true upon
        taking out the battery and replacing it.
    """
    # to do: check if we can start in other modes...
 
    def __init__(self, PORT, startingMode=OFF_MODE):
        """ the constructor which tries to open the
            connection to the robot at port PORT
        """
        # LED states stored in a dictionary
        # they start at all zeros
        self.led = {}
        self.led[ LED_STATUS ] = 0
        self.led[ LED_SPOT ] = 0
        self.led[ LED_CLEAN ] = 0
        self.led[ LED_MAX ] = 0
        self.led[ LED_DIRTDETECT ] = 0
        self.led[ LED_POWER ] = (0, 0)

        # motor states also stored in a dictionary
        # they also start at all zeros
        self.motors = {}
        self.motors[ MOTOR_MAINBRUSH ] = 0
        self.motors[ MOTOR_VACUUM ] = 0
        self.motors[ MOTOR_SIDEBRUSH ] = 0

        # here are the variables that constitute the robot's
        # estimated odometry, thr is theta in radians...
        # these are updated by integrateNextOdometricStep
        # which is called in interpretSensorString
        self.xPose =   0.0
        self.yPose =   0.0
        self.thrPose = 0.0

        # this will (try to) track the SCI mode of the robot
        #
        # OFF_MODE: at power up
        # PASSIVE_MODE: read sensors and execute virtual button presses
        #               after a virtual button press
        # SAFE_MODE: control, but safety features still on
        # FULL_MODE: complete control ... stay close!
        self.sciMode = startingMode

        # to do: allow different OS serial ports
        # to do: find the shortest safe timeout value...
        # to do: use the timeout to do more error checking than
        #        is currently done...
        #
        # the -1 here is because windows starts counting from 1
        # in the hardware control panel, but not in pyserial, it seems

        # if PORT is the string 'simulated' (or any string for the moment)
        # we use our SRSerial class
        if type(PORT) == type('string'):
            if PORT[0:2] == 'sim':
                self.ser = SRSerial('mapSquare.txt')
            else:
                # for Mac/Linux - use whole port name
                self.ser = serial.Serial(PORT, baudrate=57600, timeout=0.25)
        # otherwise, we try to open the real serial port...
        else:
            self.ser = serial.Serial(PORT-1, baudrate=57600, timeout=0.5)

        # did the serial port actually open?
        if self.ser.isOpen():
            print 'Serial port open'
        else:
            print 'Serial port did NOT open, check the'
            print '  - port number'
            print '  - physical connection'
            print '  - baud rate of the roomba (it\'s _possible_, if unlikely,'
            print '              that it might be set to 19200 instead'
            print '              of the default 57600 - removing and'
            print '              reinstalling the battery should reset it.'
 

    def getPoseCM(self):
        """ getPose returns the current estimate of the
            robot's global pose
        """
        # convert from mm to cm
        return (self.xPose/10.0, self.yPose/10.0, self.thrPose)
        
    def getPoseMM(self):
        """ getPose returns the current estimate of the
            robot's global pose
        """
        # convert from mm to cm
        return (self.xPose, self.yPose, self.thrPose)

    def setPoseMM(self, x, y, thr):
        """ setPose sets the internal odometry to the input values
              input x: global x in mm
              input y: global y in mm
              input thr: global thr in radians
        """
        self.xPose = x
        self.yPose = y
        self.thrPose = thr
        
    def setPoseCM(self, x, y, thr):
        """ setPose sets the internal odometry to the input values
              input x: global x in cm
              input y: global y in cm
              input thr: global thr in radians
        """
        self.xPose = x*10.0
        self.yPose = y*10.0
        self.thrPose = thr

    def resetPose(self):
        """ resetPose simply sets the internal odometry to 0,0,0
        """
        self.setPoseMM(0.0,0.0,0.0)

    def integrateNextOdometricStep(self, distance, rawAngle):
        """ integrateNextOdometricStep adds the reported inputs
              distance in mm
              rawAngle in mm 
            to the estimate of the robot's global pose
        """
        # OK, so this _should_ be easy
        # distance is, supposedly, the arc length that the center
        #              of the robot has traveled (the average of
        #              the two wheel's linear distances)
        #
        # rawAngle is, supposedly, the (RightWheel-LeftWheel)/2.0
        #
        # the distance (diameter) between the two wheels is 258mm
        #     keep in mind that the robot's physical diameter is larger ~
        #
        # 0.5*258 == 129mm radius
        #
        # perhaps there's nothing to do...
        if distance == 0 and rawAngle == 0:
            return

        # then again, mayber there is something to do...
        dthr = rawAngle / 129.0   # angle traveled in radians
        dthr = rawAngle / 129.0   # angle traveled in radians
        d = distance              # distance traveled
        # compute offsets in the local coordinate system,
        # with the x-axis pointing in the direction the robot was
        # initially facing (for this move) and the y-axis pointing
        # perpendicularly to the left (when facing forward)
        #
        # first, the special case when the angle is zero...
        if rawAngle == 0:
            dx = float(d)
            dy = 0.0
        # or if the distance is zero...
        elif distance == 0:
            dx = 0.0
            dy = 0.0
        # or when neither is zero...
        else:
            # finite radius of curvature
            ROC = float(d)/dthr   # remember, this is signed!
            dx = ROC*math.sin(dthr)       # because ROC is signed, 
            dy =  ROC-ROC*math.cos(dthr)  # we don't need two cases
        #
        # we need to add dx, dy, and dthr to the global pose
        # and so we need to do so in the global direction in
        # which the robot was facing at the start of this movement
        #
        # here is the unit vector describing that direction
        unitForwardX = math.cos( self.thrPose )
        unitForwardY = math.sin( self.thrPose )
        # here is the unit vector perpendicular to the left
        unitPerpX = math.cos( self.thrPose + math.pi/2.0 )
        unitPerpY = math.sin( self.thrPose + math.pi/2.0 )
        # now we compute our global offsets
        dx_global = dx*unitForwardX + dy*unitPerpX
        dy_global = dx*unitForwardY + dy*unitPerpY
        ##print 'distance and rawAngle', distance, rawAngle
        ##print 'local offsets, x, y, thd', dx, dy, math.degrees(dthr)
        ##print 'global offsets, x, y, thd', dx_global, dy_global, math.degrees(dthr)
        # and we add them all in...
        self.xPose += dx_global
        self.yPose += dy_global
        self.thrPose += dthr
        #print 'final pose', self.xPose, self.yPose, self.thrPose
        return

    def integrateNextOdometricStepCreate(self, distance, rawAngle):
       """ integrateNextOdometricStep adds the reported inputs
             distance in mm
             rawAngle in degrees
           to the estimate of the robot's global pose
       """
       # OK, so this _should_ be easy
       # distance is, supposedly, the arc length that the center
       #              of the robot has traveled (the average of
       #              the two wheel's linear distances)
       #
       # rawAngle is, supposedly, the (RightWheel-LeftWheel)/2.0
       #
       # the distance (diameter) between the two wheels is 258mm
       #     keep in mind that the robot's physical diameter is larger ~
       #
       # 0.5*258 == 129mm radius
       #
       # perhaps there's nothing to do...
       if distance == 0 and rawAngle == 0:
           return

       # then again, mayber there is something to do...
       dthr = math.radians(rawAngle)  # angle traveled
       d = distance              # distance traveled
       # compute offsets in the local coordinate system,
       # with the x-axis pointing in the direction the robot was
       # initially facing (for this move) and the y-axis pointing
       # perpendicularly to the left (when facing forward)
       #
       # first, the special case when the angle is zero...
       if rawAngle == 0:
           dx = float(d)
           dy = 0.0
       # or if the distance is zero...
       elif distance == 0:
           dx = 0.0
           dy = 0.0
       # or when neither is zero...
       else:
           # finite radius of curvature
           ROC = float(d)/dthr   # remember, this is signed!
           dx = ROC*math.sin(dthr)       # because ROC is signed,
           dy =  ROC-ROC*math.cos(dthr)  # we don't need two cases
       #
       # we need to add dx, dy, and dthr to the global pose
       # and so we need to do so in the global direction in
       # which the robot was facing at the start of this movement
       #
       # here is the unit vector describing that direction
       unitForwardX = math.cos( self.thrPose )
       unitForwardY = math.sin( self.thrPose )
       # here is the unit vector perpendicular to the left
       unitPerpX = math.cos( self.thrPose + math.pi/2.0 )
       unitPerpY = math.sin( self.thrPose + math.pi/2.0 )
       # now we compute our global offsets
       dx_global = dx*unitForwardX + dy*unitPerpX
       dy_global = dx*unitForwardY + dy*unitPerpY
       ##print 'distance and rawAngle', distance, rawAngle
       ##print 'local offsets, x, y, thd', dx, dy, math.degrees(dthr)
       ##print 'global offsets, x, y, thd', dx_global, dy_global,math.degrees(dthr)
       # and we add them all in...
       self.xPose += dx_global
       self.yPose += dy_global
       self.thrPose += dthr
       #print 'final pose', self.xPose, self.yPose, self.thrPose
       return


    def Go( self, cm_per_sec=0, deg_per_sec=0 ):
        """ Go sets the velocity in cm/sec and deg/sec """
        # need to convert to the roomba's drive parameters
        #
        # for now, just one or the other...
        if cm_per_sec == 0:
            # just handle rotation
            # convert to radians
            rad_per_sec = math.radians(deg_per_sec)
            # make sure the direction is correct
            if rad_per_sec >= 0:  dirstr = 'CCW'
            else: dirstr = 'CW'
            # compute the velocity, given that the robot's
            # radius is 258mm/2.0
            vel_mm_sec = math.fabs(rad_per_sec) * (258.0/2.0)
            # send it off to the robot
            self.drive( vel_mm_sec, 0, dirstr )

        elif deg_per_sec == 0:
            # just handle forward/backward translation
            vel_mm_sec = 10.0*cm_per_sec
            big_radius = 32767
            # send it off to the robot
            self.drive( vel_mm_sec, big_radius )

        else:
            # move in the appropriate arc
            rad_per_sec = math.radians(deg_per_sec)
            vel_mm_sec = 10.0*cm_per_sec
            radius_mm = vel_mm_sec / rad_per_sec
            # check for extremes
            if radius_mm > 32767: radius_mm = 32767
            if radius_mm < -32767: radius_mm = -32767
            self.drive( vel_mm_sec, radius_mm )

        return
 

    def __str__(self):
        """ for printing the robot's state """
        s = ''
        s += 'serial connection:' + str(self.ser) + '\n'
        s += 'robot sci mode:' + modeStr(self.sciMode) + '\n'
        # odometry
        # timestamp
        # last sensor frame
        # other stuff
        return s
 
    def start(self):
        """ changes from OFF_MODE to PASSIVE_MODE """
        self.ser.write( START )
        # they recommend 20 ms between mode-changing commands
        time.sleep(0.03)
        # change the mode we think we're in...
        self.sciMode = PASSIVE_MODE
        # no response here, so we don't get any...
	#
	# this seems to turn on the spot, clean, and power LEDs,
	# so, let's set out state to reflect that fact...
        self.led[ LED_POWER ] = (255, 0)
        self.led[ LED_CLEAN ] = 1
        self.led[ LED_SPOT ] = 1
        return

    def turnOnRTSPower(self):
        """ turnOnRTSPower wakes up the roomba the iRobot-recommended
            way of pulling the serial RTS line low for a while
            This is how the RooStick and RooRS232 work.
        """
        print 'Figure out how to use Serial to set the RTS low...'

    def turnOnRooToothPower(self):
        """ the RooTooth Bluetooth radio has its PIO pin 6
            attached to the RTS serial line for waking the
            roomba up...
        """
        self.ser.write('+++\r')  # go to AT mode
        self.ser.readline()      # reads the '\r\n'
        self.ser.readline()      # reads the 'OK\r\n'
        self.ser.write('ATSW22,6,1,1\r')  # set pin 6 to output
        self.ser.readline()      # reads the '\r\n'
        self.ser.readline()      # reads the 'OK\r\n'
        self.ser.write('ATSW23,6,0,1\r')  # set pin 6 to output
        self.ser.readline()      # reads the '\r\n'
        self.ser.readline()      # reads the 'OK\r\n'
        self.ser.write('ATSW23,6,1,1\r')  # set pin 6 to output
        self.ser.readline()      # reads the '\r\n'
        self.ser.readline()      # reads the 'OK\r\n'
        self.ser.write('ATMD\r')  # set pin 6 to output
        self.ser.readline()      # reads the '\r\n'
        self.ser.readline()      # reads the 'OK\r\n'

    def close(self):
        """ this used to turn off the power, but
            let's try just closing the serial port
            and see if we can reconnect in the previous state...
        """
        # is there other clean up to be done?
        #self.ser.write( POWER )
        # they recommend 20 ms between mode-changing commands
        time.sleep(0.03)
        # change the mode we think we're in...

        # need to track state...
        #self.sciMode = PASSIVE_MODE
        # no response here, so we don't get any...

        # close the serial port
        self.ser.close()
        return

    def closeSer(self):
        """ just disconnects the serial port """
        self.ser.close()
        return

    def openSer(self):
        """ opens the port again """
        self.ser.open()
        return


    def drive(self, roomba_mm_sec, roomba_radius_mm, turn_dir='CCW'):
        """ implements the drive command as specified
            the turn_dir should be either 'CW' or 'CCW' for
            clockwise or counterclockwise - this is only
            used if roomba_radius_mm == 0 (or rounds down to 0)
            other drive-related calls are available
        """
        # first, they should be ints
        #   in case they're being generated mathematically
        if type(roomba_mm_sec) != type(42):
            roomba_mm_sec = int(roomba_mm_sec)
        if type(roomba_radius_mm) != type(42):
            roomba_radius_mm = int(roomba_radius_mm)

        # we check that the inputs are within limits
        # if not, we cap them there
        if roomba_mm_sec < -500:
            roomba_mm_sec = -500
        if roomba_mm_sec > 500:
            roomba_mm_sec = 500

        # if the radius is beyond the limits, we go straight
	# it doesn't really seem to go straight, however...
        if roomba_radius_mm < -2000:
            roomba_radius_mm = 32768
        if roomba_radius_mm > 2000:
            roomba_radius_mm = 32768
        
        # get the two bytes from the velocity
        # these come back as numbers, so we will chr them
        velHighVal, velLowVal = toTwosComplement2Bytes( roomba_mm_sec )

        # get the two bytes from the radius in the same way
        # note the special cases
        if roomba_radius_mm == 0:
            if turn_dir == 'CW':
                roomba_radius_mm = -1
            else: # default is 'CCW' (turning left)
                roomba_radius_mm = 1
        radiusHighVal, radiusLowVal = toTwosComplement2Bytes( roomba_radius_mm )

        #print 'bytes are', velHighVal, velLowVal, radiusHighVal, radiusLowVal

        # send these bytes and set the stored velocities
        self.ser.write( DRIVE )
        self.ser.write( chr(velHighVal) )
        self.ser.write( chr(velLowVal) )
        self.ser.write( chr(radiusHighVal) )
        self.ser.write( chr(radiusLowVal) )


    def setLED(self, led_in, state, optstate=0):
	""" sets led off (if state is 0) and on (if state is 1)
	    
	    led can be roomba.LED_STATUS
	               roomba.LED_SPOT
	               roomba.LED_CLEAN
	               roomba.LED_MAX
	               roomba.LED_DIRTDETECT
		       roomba.LED_POWER

            for most of them, state can be 0 or 1
            for roomba.LED_STATUS, state can be 0-3
	    for roomba.LED_POWER,  state can be 0-255 (intensity)
	                    and optstate can be 0-255 (color)
        """
        # change the dictionary appropriately
        # then call setAbsoluteLEDs on the dictionary

        # should error-check here!
        if led_in == LED_POWER:
            self.led[ led_in ] = (state, optstate)

        else:
            self.led[ led_in ] = state

        self.setDictionaryLEDs()




    def setDictionaryLEDs(self):
        """ this calls setAbsoluteLEDs with the contents of the
            robot's current dictionary of led states
        """
        poweramt, powercoloramt = self.led[ LED_POWER ]
        self.setAbsoluteLEDs( status=self.led[ LED_STATUS ], \
                              spot=self.led[ LED_SPOT ], \
                              clean=self.led[ LED_CLEAN ], \
                              max=self.led[ LED_MAX ], \
                              dirtdetect=self.led[ LED_DIRTDETECT ], \
                              power=poweramt, \
                              powercolor=powercoloramt )



    def setAbsoluteLEDs(self, status=0, spot=0, clean=0, max=0, dirtdetect=0, power=0, powercolor=0):
        """ this function implements the LED control directly
            there should be individual components implemented, too

            the inputs all default to 0; they can be
              status = 0 (off), 1 (red), 2 (green), 3 (amber)
              spot = 0 (off), 1 (on)
              clean = 0 (off), 1 (on)
              max = 0 (off), 1 (on)
              dirtdetect = 0 (off), 1 (on)
              power = 0 to 255 (intensity)
              powercolor = 0 to 255 (color: 0 is green, 255 is red)
        """
        # make sure we're within range...
        if status != 1 and status != 2 and status != 3: status = 0
        if spot != 0: spot = 1
        if clean != 0: clean = 1
        if max != 0: max = 1
        if dirtdetect != 0: dirtdetect = 1
        try:
            power = int(power)
            powercolor = int(powercolor)
        except TypeError:
            power = 128
            powercolor = 128
            print 'Excpetion caught in setAbsoluteLEDs in roomba.py'
        if power < 0: power = 0
        if power > 255: power = 255
        if powercolor < 0: powercolor = 0
        if powercolor > 255: powercolor = 255
        # create the first byte
        firstByteVal = (status << 4) | (spot << 3) | (clean << 2) | (max << 1) | dirtdetect

        # send these as bytes
        # print 'bytes are', firstByteVal, powercolor, power

        # send these bytes and set the stored velocities
        self.ser.write( LEDS )
        self.ser.write( chr(firstByteVal) )
        self.ser.write( chr(powercolor) )
        self.ser.write( chr(power) )

        return


    # 
    # DO NOT CALL THIS FUNCTION!
    #    call readSensors instead - it will integrate odometry
    #    for what that's worth, admittedly...
    #    if you call this without integrating odometry, the
    #    distance and rawAngle reported will be lost...
    # 
    def getRawSensorData(self, packetnumber):
        """ gets back a raw string of sensor data
            which then can be used to create a SensorFrame
        """
        if packetnumber != 1 and packetnumber != 2 and packetnumber != 3: 
            packetnumber = 0

        self.ser.write( SENSORS )
        self.ser.write( chr(packetnumber) )

        if packetnumber == 0:
            r = self.ser.read(size=26)
        if packetnumber == 1:
            r = self.ser.read(size=10) + 16*chr(0)
        if packetnumber == 2:
            r = 10*chr(0) + self.ser.read(size=6) + 10*chr(0)
        if packetnumber == 3:
            r = 16*chr(0) + self.ser.read(size=10)


        # need error checking or sanity checking or something!
        if r == "" or r == None:
            print "ERROR: unable to read sensor data from Roomba"

        return r

    def seekDock(self):
	""" sends the force-seeking-dock signal
	    it must be in a cleaning mode to do this, 
	    so we (virtually) press the clean button first,
	    and then wait a bit...
	"""
        self.ser.write( CLEAN )
	time.sleep(2.0)  # wait for two seconds
        self.ser.write(FORCESEEKINGDOCK)
        self.sciMode = PASSIVE_MODE
	return 


    def setSong(self, songNumber, songDataList):
        """ this stores a song to roomba's memory to play later
            with the playSong command

            songNumber must be between 0 and 15 (inclusive)
            songDataList is a list of (note, duration) pairs (up to 16)
                         note is the midi note number, from 31 to 127
                              (outside this range, the note is a rest)
                         duration is from 0 to 255 in 1/64ths of a second
        """
        # any notes to play?
        if len(songDataList) < 1:
            print 'No data in the songDataList'
            return

        if songNumber < 0: songNumber = 0
        if songNumber > 15: songNumber = 15

        # indicate that a song is coming
        self.ser.write( SONG )
        self.ser.write( chr(songNumber) )

        L = min(len(songDataList), 16)
        self.ser.write( chr(L) )

        # loop through the notes, up to 16
        for note in songDataList[:L]:
            # make sure its a tuple, or else we rest for 1/4 second
            if type(note) == type( () ):
                #more error checking here!
                self.ser.write( chr(note[0]) )  # note number
                self.ser.write( chr(note[1]) )  # duration
            else:
                self.ser.write( chr(30) )   # a rest note
                self.ser.write( chr(16) )   # 1/4 of a second

        # no change to the SCI mode
        return


    def playSong(self, songNumber):
        """ plays song songNumber """
        #
        if songNumber < 0: songNumber = 0
        if songNumber > 15: songNumber = 15

        self.ser.write( PLAY )
        self.ser.write( chr(songNumber) )

    def playNote(self, noteNumber, duration, songNumber=0):
        """ plays a single note as a song at songNumber """
        # set the song
        self.setSong(songNumber, [(noteNumber,duration)])
        self.playSong(songNumber)

    def setMainBrush(self, state):
        """ turns mainbrush motor on (state==1) or off (state==0) """
        if state: state = 1
        else: state = 0
        self.motors[MOTOR_MAINBRUSH] = state
        self.setDictionaryMotors()

    def setVacuum(self, state):
        """ turns mainbrush motor on (state==1) or off (state==0) """
        if state: state = 1
        else: state = 0
        self.motors[MOTOR_VACUUM] = state
        self.setDictionaryMotors()

    def setSideBrush(self, state):
        """ turns mainbrush motor on (state==1) or off (state==0) """
        if state: state = 1
        else: state = 0
        self.motors[MOTOR_SIDEBRUSH] = state
        self.setDictionaryMotors()

    def setDictionaryMotors(self):
        """ we set the motors according to our dictionary """
        self.setAbsoluteMotors( mainbrush=self.motors[ MOTOR_MAINBRUSH ], \
                                vacuum=self.motors[ MOTOR_VACUUM ], \
                                sidebrush=self.motors[ MOTOR_SIDEBRUSH ] )

    def setAbsoluteMotors(self, mainbrush=0, vacuum=0, sidebrush=0):
        """ sets each motor on (1) or off (0) """
        # should check if in Safe or Full mode!!

        # make sure each is 1 or 0
        if mainbrush: mainbrush = 1
        else: mainbrush = 0
        if vacuum: vacuum = 1
        else: vacuum = 0
        if sidebrush: sidebrush = 1
        else: sidebrush = 0

        # set the motors' state
        self.ser.write( MOTORS )
        self.ser.write( chr( (mainbrush << 2) | (vacuum << 1) | sidebrush ) )


    def readSensors(self, frame=0):
        """ this gets the sensor data in the specified frame and 
                frame 0 == all data
                frame 1 == bumps and wheeldrops (urgent)
                frame 2 == buttons and odometry (medium)
                frame 3 == electrical status (lower frequency)
            returns a (complete) SensorFrame with that data
        """
        r = self.getRawSensorData(frame)
        s = self.interpretSensorString(r)
        return s

    def getData(self):
        """ for class """
        s = self.readSensors()
        return (s.distance, math.degrees(s.angleInRadians), s.leftBump, s.rightBump)


    def buttonPress(self, button):
        """ this "presses" (virtually) one of the roomba's
            buttons. The robot goes into PASSIVE_MODE and
            it will start the button's specified behavior

            button should be one of
            roomba.POWER
            roomba.SPOT
            roomba.CLEAN or
            roomba.MAX
        """
        if button == POWER:
            self.ser.write( POWER )
        elif button == SPOT:
            self.ser.write( SPOT )
        elif button == CLEAN:
            self.ser.write( CLEAN )
        elif button == MAX:
            self.ser.write( MAX )
        else:
            print 'The button', button, 'in buttonPress was not recognized.'
            print 'Not sending anything to the robot.'
            return

        # the robot should go into passive mode
        self.sciMode = PASSIVE_MODE
        # they recommend 20 ms between mode-changing commands
        time.sleep(0.03)
        # no response expected
        return


    def powerOff(self):
        """ pressed the power button (virtually) """
        self.buttonPress( POWER )

        
    def toFullMode(self):
        """ changes the state (from SAFE_MODE) to FULL_MODE
        """
        if self.sciMode == SAFE_MODE:
            self.ser.write( FULL )
            # they recommend 20 ms between mode-changing commands
            time.sleep(0.03)
            # change the mode we think we're in...
            self.sciMode = FULL_MODE
            # no response here, so we don't get any...
            return

        print 'Going to FULL_MODE from', modeStr(self.sciMode),
        print 'not permitted.'
        print 'Not sending anything.'
        return


    def toSafeMode(self):
        """ changes the state (from PASSIVE_MODE or FULL_MODE)
            to SAFE_MODE
        """
        if self.sciMode == PASSIVE_MODE:
            self.ser.write( CONTROL )
            # they recommend 20 ms between mode-changing commands
            time.sleep(0.03)
            # change the mode we think we're in...
            self.sciMode = SAFE_MODE
            # no response here, so we don't get any...
            return

        if self.sciMode == FULL_MODE:
            self.ser.write( SAFE )
            # they recommend 20 ms between mode-changing commands
            time.sleep(0.03)
            # change the mode we think we're in...
            self.sciMode = SAFE_MODE
            # no response here, so we don't get any...
            return

        if self.sciMode == OFF_MODE:
            ##print 'Going to SAFE_MODE from OFF_MODE not permitted.'
            # why not?
            # we will just send both commands
            self.start()
            # now we're in PASSIVE_MODE, so we repeat the above code...
            self.ser.write( CONTROL )
            # they recommend 20 ms between mode-changing commands
            time.sleep(0.03)
            # change the mode we think we're in...
            self.sciMode = SAFE_MODE
            # no response here, so we don't get any...
            return

        if self.sciMode == SAFE_MODE:
            # print 'Already in SAFE_MODE'
            return
            


    def getMode(self):
        """ returns one of OFF_MODE, PASSIVE_MODE, SAFE_MODE, FULL_MODE """
        # but how right is it?
        return self.sciMode


    def setBaudRate(self, baudrate=10):
        """ sets the communications rate to the desired value """
        # check for OK value
        #baudcode = 10  # 57600, the default
        if baudrate == 300: baudcode = 0
        elif baudrate == 600: baudcode = 1
        elif baudrate == 1200: baudcode = 2
        elif baudrate == 2400: baudcode = 3
        elif baudrate == 4800: baudcode = 4
        elif baudrate == 9600: baudcode = 5
        elif baudrate == 14400: baudcode = 6
        elif baudrate == 19200: baudcode = 7
        elif baudrate == 28800: baudcode = 8
        elif baudrate == 38400: baudcode = 9
        elif baudrate == 57600: baudcode = 10
        elif baudrate == 115200: baudcode = 11
        else:
            print 'The baudrate of', baudrate, 'in setBaudRate'
            print 'was not recognized. Not sending anything.'
            return
        # otherwise, send off the message
        self.ser.write( START )
        self.ser.write( chr(baudcode) )
        # the recommended pause
        time.sleep(0.1)
        # change the mode we think we're in...
        self.sciMode = PASSIVE_MODE
        # no response here, so we don't get any...
        return


    def interpretSensorString( self, r ):
        """ This returns a sensorFrame object with its fields
            filled in from the raw sensor return string, r, which
            has to be the full 3-packet (26-byte) string. 
    
            r is obtained by writing [142][0] to the serial port.
        """
        # check length
        # we should save a bit of time by handling each sub-string
        # appropriately, but we don't do this yet...
        if len(r) != 26:
            #print 'You have input an incorrectly formatted string to'
            #print 'sensorStatus. It needs to have 26 bytes (full sensors).'
            #print 'The input is', r
            return
    
        s = SensorFrame()
    
        # convert r so that it is a list of 26 ints instead of 26 chrs
        r = [ ord(c) for c in r ]
    
        # packet number 1 (10 bytes)
    
        # byte 0: bumps and wheeldrops
        s.casterDrop = bitOfByte( 4, r[0] )
        s.leftWheelDrop = bitOfByte( 3, r[0] )
        s.rightWheelDrop = bitOfByte( 2, r[0] )
        s.leftBump = bitOfByte( 1, r[0] )
        s.rightBump = bitOfByte( 0, r[0] )
    
        # byte 1: wall sensor, the IR looking to the right
        s.wallSensor = bitOfByte( 0, r[1] )
    
        # byte 2: left cliff sensor
        s.leftCliff = bitOfByte( 0, r[2] )
    
        # byte 3: front left cliff sensor
        s.frontLeftCliff = bitOfByte( 0, r[3] )
    
        # byte 4: front right cliff sensor
        s.frontRightCliff = bitOfByte( 0, r[4] )
    
        # byte 5: right cliff sensor
        s.rightCliff = bitOfByte( 0, r[5] )
    
        # byte 6: virtual wall detector (the separate unit)
        s.virtualWall = bitOfByte( 0, r[6] )
    
        # byte 7: motor overcurrents byte
        s.driveLeft = bitOfByte( 4, r[7] )
        s.driveRight = bitOfByte( 3, r[7] )
        s.mainBrush = bitOfByte( 2, r[7] )
        s.vacuum = bitOfByte( 1, r[7] )
        s.sideBrush = bitOfByte( 0, r[7] )
    
        # byte 8: dirt detector left
        # the dirt-detecting sensors are acoustic impact sensors
        # basically, they hear the dirt (or don't) going by toward the back
        # this value ranges from 0 (no dirt) to 255 (lots of dirt)
        s.leftDirt = r[8]
    
        # byte 9: dirt detector right
        # some roomba's don't have the right dirt detector
        # the dirt detectors are metallic disks near the brushes
        s.rightDirt = r[9]
    
        # packet number 2 (6 bytes)
    
        # byte 10: remote control command
        # this is the value of the remote control command currently
        # being seen by the roomba, it is 255 if there is no command
        # not all roombas have a remote control...
        s.remoteControlCommand = r[10]
    
        # byte 11: button presses
        s.powerButton = bitOfByte( 3, r[11] )
        s.spotButton = bitOfByte( 2, r[11] )
        s.cleanButton = bitOfByte( 1, r[11] )
        s.maxButton = bitOfByte( 0, r[11] )
    
        # bytes 12 and 13: distance
        # the distance that roomba has traveled, in mm, since the
        # last time this data was requested (not from a SensorFrame,
        # but from the roomba)
        # It will stay at the max or min (32767 or -32768) if
        # not polled often enough, i.e., it then means "a long way"
        # It is the sum of the two drive wheels' distances, divided by 2
        s.distance = twosComplementInt2bytes( r[12], r[13] )
    
        # bytes 14 and 15: angle
        s.rawAngle = twosComplementInt2bytes( r[14], r[15] )
        # the distance between the wheels is 258 mm
        #s.angleInRadians = 2.0 * s.rawAngle / 258.0
        s.angleInRadians = math.radians(s.rawAngle)
        # packet number 3 (10 bytes)
    
        # byte 16: charging state
        # 0 == not charging
        # 1 == charging recovery
        # 2 == charging
        # 3 == trickle charging
        # 4 == waiting
        # 5 == charging error
        s.chargingState = r[16]
    
        # bytes 17 and 18: voltage in millivolts
        # this is unsigned, so we don't use two's complement
        s.voltage = r[17] << 8 | r[18]
        # check this for byte order!!
    
        # bytes 19 and 20: current in milliamps
        # this is signed, from -32768 to 32767
        # negative currents are flowing out of the battery
        # positive currents are flowing into the battery (charging)
        s.current = twosComplementInt2bytes( r[19], r[20] )
    
        # byte 21: temperature of the battery
        # this is in degrees celsius
        s.temperature = twosComplementInt1byte( r[21] )
    
        # bytes 22 and 23: charge of the battery in milliamp-hours
        # this is two unsigned bytes
        s.charge = r[22] << 8 | r[23]
    
        # bytes 24 and 25: estimated capacity of the roomba's battery
        # in units of milliamp-hours
        # when the charge reaches this value, the battery is
        # considered fully charged
        s.capacity = r[24] << 8 | r[25]

        # OK, here we call a function to integrate the odometric
        # step taken here (unless distance and rawAngle are 0)
        self.integrateNextOdometricStepCreate(s.distance,s.rawAngle)
    
        return s


class RoombaRobot:
    """ this class contains a Roomba object to handle all of the
        communications, but this class actually starts to reason
        about the robot, e.g., where it is right now...
    """
    def __init__(self, PORT, mapfilename=None):
        """ the constructor creates the robot object 
            and tracks its position
        """
        self.roomba = Roomba(PORT)
        if mapfilename != None:
            self.roomba.ser.setMap(mapfilename)
        # in cm and radians
        self.x = 0.0
        self.y = 0.0
        self.thr = 0.0

    def SetPose(self, x, y, thr):
        """ set's the robot's global pose to x, y, thr
        """
        self.x = x
        self.y = y
        self.thr = thr
        # now set the simulator's pose there, too!
        self.roomba.ser.x_real = x*10.0
        self.roomba.ser.y_real = y*10.0
        self.roomba.ser.thr_real = thr

    def SetCollisions(self, doWeCrashIntoWalls):
        """ sets the simulator to use or ignore walls
        """
        self.roomba.ser.doWeCrashIntoWalls = doWeCrashIntoWalls

    def GetPose(self):
        """ cheats by providing the internal pose... """
        s = self.roomba.readSensors() # updates the simulator's pose
        self.x = self.roomba.ser.x_real/10.0 # and then grabs it...
        self.y = self.roomba.ser.y_real/10.0
        self.thr = self.roomba.ser.thr_real
        # get the two bump sensors
        lb = s.leftBump
        rb = s.rightBump
        return (self.x, self.y, self.thr, lb, rb)

    def GetPoseReal(self):
        """ GetPose updates the pose and returns it """
        # s is a SensorFrame
        s = None
        while s == None:
            s = self.roomba.readSensors()
            if s == None:
                print "WARNING: readSensors failed, retrying..."


        # we have the distance and angle
        d = s.distance/10.0  # the distance the center traveled in cm
        thr = s.angleInRadians  # the angle the robot changed

        # this means we know the arc of the circle it's traveled...
        # in robot-local coordinates here
        if thr == 0.0 or math.fabs(d/thr) > 10000:
            # just went straight
            x_delta = d
            y_delta = 0.0
        else:
            ROC = d/float(thr)
            x_delta = 0.0 + ROC*math.sin(thr)
            y_delta = ROC - ROC*math.cos(thr)

        x_from_deltax = math.cos(self.thr) * x_delta
        y_from_deltax = math.sin(self.thr) * x_delta
        x_from_deltay = math.cos(self.thr + math.pi/2.0) * y_delta
        y_from_deltay = math.sin(self.thr + math.pi/2.0) * y_delta

        self.x = self.x + x_from_deltax + x_from_deltay
        self.y = self.y + y_from_deltax + y_from_deltay
        self.thr += thr

        # find our deltas in a canonical coordinate system
        return (self.x, self.y, self.thr, s.leftBump, s.rightBump)


    def Go( self, cm_per_sec, deg_per_sec ):
        """ Go sets the velocity in cm/sec and deg/sec """
        # need to convert to the roomba's drive parameters
        #
        # for now, just one or the other...
        if cm_per_sec == 0:
            # just handle rotation
            # convert to radians
            rad_per_sec = math.radians(deg_per_sec)
            # make sure the direction is correct
            if rad_per_sec >= 0:  dirstr = 'CCW'
            else: dirstr = 'CW'
            # compute the velocity, given that the robot's
            # radius is 258mm/2.0
            vel_mm_sec = math.fabs(rad_per_sec) * (258.0/2.0)
            # send it off to the robot
            self.roomba.drive( vel_mm_sec, 0, dirstr )

        elif deg_per_sec == 0:
            # just handle forward/backward translation
            vel_mm_sec = 10.0*cm_per_sec
            big_radius = 30000
            # send it off to the robot
            self.roomba.drive( vel_mm_sec, big_radius )


        else:
            #
            print 'Go in RoombaRobot not implemented yet'

        return


import sensorSimulator

#
# the SRSerial class: "simulated roomba serial"
#
class SRSerial:
    """ this will emulate the serial connection to a
        simulated robot
    """

    def __init__(self, mapfilename):
        """ this constructor creates data members sufficient to
            track the state of a roomba and starts a timer, so that
            odometry, etc. can be updated with each call

            a map is needed: the simulated robot has to be somewhere...
        """
        # perhaps a default empty map would be good to have?
        self.map = sensorSimulator.Map( mapfilename )
        self.doWeCrashIntoWalls = True # by default, we use the walls...

        # WHAT STATE DO WE NEED?
        # SONGS - do this later
        # SCI STATE - no, you can't query it!
        # LEDs - no, you can't query them
        # BUTTONS, MOTORS, CURRENT, etc. - do this later, if at all
        # VELOCITIES - we'll use the speed/radius representation now
        self.velocity = 0.0
        self.radius = 0.0
        # POSITION - we'll keep the pose of the robot here for now
        # there's something odd about calling these simulated positions
        # x_real, y_real, and thr_real, but since we will be allowing
        # for the addition of simulated error models, we'll just go with it...
        self.x_real = 0.0
        self.y_real = 0.0
        self.thr_real = 0.0

        # not that odometric computations used for decision-making
        # should be in Roomba, not SRSerial: none of the above data is
        # accessible to the real-roomba programmer

        # keep track of the last update time
        self.lastUpdateTime = time.clock()

        # keep a SensorFrame around to store sensor readings, etc.
        self.sensorframe = SensorFrame()

        # keep track of whether the user is in the midst of writing
        # a string of bytes that will (end up) being a robot command
        #
        # this data member holds the command being written so far
        #    the first element, cmdSoFar[0], will be a byte, e.g. DRIVE = chr(137)
        #    subsequent elements will be the ordinal (int) values following it
        #
        # if there is no string in progress, it holds [] (which is False)
        #
        self.cmdSoFar = []

        # set to default state, e.g., velocities to 0.0
        # start timer anew
        self.reset()

    # this should be done so that the interface is natural
    # with the Roomba and Roomba simulator
    def setMap(self, mapfilename):
        """ allows us to set a different map for this simulator
        """
        self.map = sensorSimulator.Map(mapfilename)
        return

    def getRealPosition(self):
        """ returns the 'real' position, i.e., the position from which
            the simulated robot is actually computing sensor values, etc.
        """
        return (self.x_real, self.y_real, self.thr_real)

    def reset(self):
        """ reset places everything into its default state, e.g.,
            velocities to 0.0, and it will also start the timer anew
            and ignore any previous bytes sent via write(...)
        """
        # need to reset self.sensorframe
        self.lastUpdateTime = time.clock()
        self.velocity = 0.0
        self.radius = 0.0

    def open(self):
        """ to allow the serial port's open call
            does nothing at the moment...
        """
        pass

    def close(self):
        """ to allow the serial port's close call
            does nothing at the moment...
        """
        pass

    def update(self):
        """ moves the robot within the map and determines what
            the robot's sensor readings should be
        """
        # to do: not all sensor readings are currently implemented
        # 
        currentTime = time.clock()
        deltaTime = currentTime - self.lastUpdateTime
        # print 'deltaTime is', deltaTime

        # we compute the new position of the robot, assuming that
        # it does not hit any obstacles
        xythrdeltas = poseDeltaFromVelRadSec( self.velocity, self.radius, deltaTime )
        # print 'xythrdeltas is', xythrdeltas

        # we need to add these robot-local deltas to the robot's global pose
        x_delta, y_delta, thr_delta = xythrdeltas

        x_from_deltax = math.cos(self.thr_real) * x_delta
        y_from_deltax = math.sin(self.thr_real) * x_delta
        x_from_deltay = math.cos(self.thr_real + math.pi/2.0) * y_delta
        y_from_deltay = math.sin(self.thr_real + math.pi/2.0) * y_delta

        x_new = self.x_real + x_from_deltax + x_from_deltay
        y_new = self.y_real + y_from_deltax + y_from_deltay
        thr_new = self.thr_real + thr_delta

        # we check to see if it hit any obstacles (or other events)
        # if so, we fix the robot's position to account for this
        if self.doWeCrashIntoWalls:
            C = self.map.findroombacollisions( x_new/10.0, y_new/10.0, thr_new )
        else:
            C = []   # no walls!

        if C:
            # the robot collided!
            #print 'C is', C
            #print 'x,y,thnew are', x_new, y_new, thr_new
            # set the collision bits...
            # we use a 30-degree threshold on either side of forward
            # for triggering both bump sensors CHECK THIS!!
            leftbump = 0
            rightbump = 0
            for c in C:
                if c > math.radians(100) or c < math.radians(-100):
                    pass # no bump sensors there CHECK THESE THRESHES!!
                elif c > math.radians(30):
                    rightbump = 1
                elif c > math.radians(-30):
                    rightbump = 1
                    leftbump = 1
                else:
                    leftbump = 1
            # we don't change the self.x_real, ...
        else:
            # the robot did not collide...
            self.x_real = x_new
            self.y_real = y_new
            self.thr_real = thr_new
            leftbump = 0
            rightbump = 0


        # we set the sensor readings in self.sensorframe appropriately
        #
        # first, we set the distance and angle!
        # we will use the real distance (center's distance)  - though
        # it is not at all clear if this is really what is happening!
        # CHECK THIS TO SEE If IT IS CORRECT!
        self.sensorframe.angleInRadians = thr_delta
        self.sensorframe.rawAngle = int(258.0*thr_delta/2.0)
        # we use the radius to find the distance
        # need to handle special cases
        if -1 <= self.radius <= 1: # really 0
            self.sensorframe.distance = 0
        elif self.radius < -2000 or self.radius > 2000: # straight
            self.sensorframe.angleInRadians = 0.0
            self.sensorframe.rawAngle = 0
            self.sensorframe.distance = self.velocity * deltaTime
        else:
            # the radius is in mm...
            self.sensorframe.distance = thr_delta * self.radius
        # make sure they're ints
        self.sensorframe.distance = int(self.sensorframe.distance)

        #
        # next, the bump sensors
        self.sensorframe.leftBump = leftbump
        self.sensorframe.rightBump = rightbump

        # we reset the last time things were updated
        self.lastUpdateTime = currentTime
        return

    def write(self,s):
        """ takes in the string being written to the roomba and
            changes the state accordingly

            if the velocities are changed, write calls update()
            after changing them so that things are simulated correctly

            write returns None
        """
        # in order to allow arbitrary physical breaking of the logical
        # "command string" that write is sending, a data member is used
        # to accumulate the correct number of bytes.
        #
        # who knows what to do if the byte sequence does not match
        # the specification - iRobot doesn't say what the roomba does
        # in such situations...
        #

        # if there's no string, do nothing
        if s == '': 
            return None

        # some feedback for debugging...
        #print 'I see a string with values', [ ord(x) for x in s ]

        # consider the first byte sent
        if self.cmdSoFar == []:
            # it's the start of a message... add the first byte seen
            self.cmdSoFar.append( s[0] ) # this has to be the byte itself

        else:
            # it's part of a previously-started message, add the first byte
            self.cmdSoFar.append( ord(s[0]) ) # the rest we convert to ints

        # OK, now let's see if we're finished with this message...
        #     We need to check all the possible messages
        if self.cmdSoFar[0] == START:
            # if we implement modes, it would go here...
            self.cmdSoFar = []  # reset 

        elif self.cmdSoFar[0] == BAUD:
            if len(self.cmdSoFar) >= 2:
                # baud command - ignore for now
                self.cmdSoFar = []  # reset 

        elif self.cmdSoFar[0] == CONTROL: 
            # if we implement modes, it would go here...
            self.cmdSoFar = []  # reset 

        elif self.cmdSoFar[0] == SAFE: 
            # if we implement modes, it would go here...
            self.cmdSoFar = []  # reset 

        elif self.cmdSoFar[0] == FULL:
            # if we implement modes, it would go here...
            self.cmdSoFar = []  # reset 

        elif self.cmdSoFar[0] == POWER:
            # if we implement buttons, it would go here...
            self.cmdSoFar = []  # reset 

        elif self.cmdSoFar[0] == SPOT:
            # if we implement buttons, it would go here...
            self.cmdSoFar = []  # reset 

        elif self.cmdSoFar[0] == CLEAN:
            # if we implement buttons, it would go here...
            self.cmdSoFar = []  # reset 

        elif self.cmdSoFar[0] == MAX:
            # if we implement buttons, it would go here...
            self.cmdSoFar = []  # reset 

        elif self.cmdSoFar[0] == DRIVE:
            if len(self.cmdSoFar) >= 5:
                velHigh = self.cmdSoFar[1]
                velLow  = self.cmdSoFar[2]
                radHigh = self.cmdSoFar[3]
                radLow  = self.cmdSoFar[4]
                velocity = twosComplementInt2bytes(velHigh,velLow)
                radius   = twosComplementInt2bytes(radHigh,radLow)
                # before setting self.speed and self.radius,
                # we update things
                self.update()
                # now we set the new velocity values
                self.velocity = velocity
                # we will be a bit more forgiving - any
                # radius outside the -2000 to 2000 range will
                # be set to 32768, which the SCI specification
                # says is "straight"
                if radius < -2000 or radius > 2000:
                    self.radius = 32768
                else:
                    self.radius = radius
                #
                # print 'setting vel, rad to', self.velocity, self.radius
                #
                self.cmdSoFar = []  # reset 

        elif self.cmdSoFar[0] == MOTORS:
            if len(self.cmdSoFar) >= 2:
                # if we implement other motors, it would go here...
                self.cmdSoFar = []  # reset 

        elif self.cmdSoFar[0] == LEDS:
            if len(self.cmdSoFar) >= 4:
                # if we implement leds, it would go here...
                self.cmdSoFar = []  # reset 

        elif self.cmdSoFar[0] == SONG:
            if len(self.cmdSoFar) >= 3:
                songnumber = self.cmdSoFar[1]
                songlength = self.cmdSoFar[2]
                if len(self.cmdSoFar) >= 2*songlength + 3:
                    # if we implement songs, it would go here...
                    self.cmdSoFar = []  # reset 
        
        elif self.cmdSoFar[0] == PLAY:
            if len(self.cmdSoFar) >= 2:
                # if we implement songs, it would go here...
                self.cmdSoFar = []  # reset 

        elif self.cmdSoFar[0] == SENSORS:
            if len(self.cmdSoFar) >= 2:
                # if we implement sensor frames, it would go here...
                self.cmdSoFar = []  # reset 

        elif self.cmdSoFar[0] == FORCESEEKINGDOCK:
            #if len(self.cmdSoFar) >= 4:
                # if we implement leds, it would go here...
                self.cmdSoFar = []  # reset 

        else:
            # the first byte was not recognized... reset
            self.cmdSoFar = []

        # handle the rest of the bytes, if there are any...
        return self.write(s[1:])


    def read(self,size):
        """ size is the number of bytes to read

            this calls update to compute what the sensors would read
            it then returns the sensor data
        """
        # todo: this only implements the 26-byte (full) frame at the moment
        self.update()
        return self.sensorframe.toBinaryString()



